from dev.mmw import MMWPointBase, MMWPoinsCloud, Frame, Frames
import os

class ARS548Point(MMWPointBase):
    '''
    x(m),y(m),z(m),InvalidFlags,RangeRate(m/S),RangeRateSTD(m/S),RCS(dBm2),MeasurementID,PositivePredictiveValue(%),Classification,MultiTargetProbability(%),ObjectID,AmbiguityFlag,SortIndex,
    '''
    def __init__(self):
        super().__init__()
        self.InvalidFlags = 1
        self.RangeRate = 0
        self.RangeRateSTD = 0
        self.RCS = 0
        self.MeasurementID = 0
        self.PositivePredictiveValue = 0
        self.Classification = 0
        self.MultiTargetProbability = 0
        self.ObjectID = 0
        self.AmbiguityFlag = 0
        self.SortIndex = 0

class ARS548Loader:
    '''
    dump mmw points data from a csv file, which is saved by the radar sensor.
    '''
    def __init__(self):
        self.__frames = Frames()
        self.__currnet_ts = 0

    def __parse_frame(self, lines):
        # get the timestamp
        ts_line = lines[0]
        # split the line by ,
        ts_parts = ts_line.split(',')
        # get the timestamp
        ts = int(ts_parts[1])
        print('start parse frame:', ts)
        # get data lines: the lines below the line including 'x(m),y(m),z(m)'
        for i in range(1, len(lines)):
            if 'x(m),y(m),z(m)' in lines[i]:
                data_lines = lines[i+1:]
                break
        # get the data
        cloud = MMWPoinsCloud()
        for line in data_lines:
            parts = line.split(',')
            point = ARS548Point()
            point.x = float(parts[0])
            point.y = float(parts[1])
            point.z = float(parts[2])
            point.InvalidFlags = int(parts[3])
            point.RangeRate = float(parts[4])
            point.RangeRateSTD = float(parts[5])
            point.RCS = float(parts[6])
            point.MeasurementID = int(parts[7])
            point.PositivePredictiveValue = float(parts[8])
            point.Classification = parts[9]
            point.MultiTargetProbability = float(parts[10])
            point.ObjectID = int(parts[11])
            point.AmbiguityFlag = int(parts[12])
            point.SortIndex = int(parts[13])
            point.ref = point.RCS
            cloud.add_point(point)
        frame = Frame(ts, cloud)
        print(frame)
        self.__frames.add_frame(frame)

    def load(self, filename):
        '''
        load mmw points data from a csv file.
        '''
        # spit the lines by empty lines
        with open(filename, 'r') as f:
            lines = []
            for line in f:
                line = line.strip()
                if line:
                    lines.append(line)
                else:
                    self.__parse_frame(lines)
                    lines = []
            if lines:
                self.__parse_frame(lines)

    def dump(self, outdir='.'):
        '''
        dump the mmw points data to files.
        '''
        ts = 0
        frame_index = 0
        for i in range(len(self.__frames)):
            frame = self.__frames.get_frame(i)
            # filename = f'{outdir}/{frame.timestamp}.pcd'
            # frame.points.save_pcd(filename)
            if ts == frame.timestamp:
                frame_index += 1
            else:
                ts = frame.timestamp
                frame_index = 0
            filename = os.path.join(outdir, f'{frame.timestamp}_{frame_index}.pcd')
            frame.points.save_pcd(filename)
